#include "naojointcontrol.h"
#include <std_msgs/String.h>


NaoJointcontrol::NaoJointcontrol(int argc, char ** argv)
{
    m_ip = "localhost";
    m_port = 9559;

    if (!  connectProxy() )
    {
        ROS_ERROR("Gosh! Throwsing exception");
        throw std::exception();
    }

    ROS_INFO("nao_jointcontrol initialized");

}

bool NaoJointcontrol::connectProxy()
{
    try
    {
        m_motionProxy = boost::shared_ptr<AL::ALMotionProxy>(new AL::ALMotionProxy(m_ip, m_port));
    }
    catch (const AL::ALError& e)
    {
        ROS_ERROR("Could not create ALMotionProxy.");
        return false;
    }
    try
    {
        m_memoryProxy = boost::shared_ptr<AL::ALMemoryProxy>(new AL::ALMemoryProxy(m_ip, m_port));
    }
    catch (const AL::ALError& e)
    {
        ROS_ERROR("Could not create ALMemoryProxy.");
        return false;
    }
    ROS_INFO("Proxies to ALMotion and ALMemory ready.");
    return true;
}

NaoJointcontrol::~NaoJointcontrol()
{
}

void NaoJointcontrol::run()
{
    std::string chainName  = "Torso";
    int space              = 2;
    std::vector<float> position(6, 0.0f); // Absolute Position
    position[1] = 0.08f;
    position[2] = 0.2f;
    float fractionMaxSpeed = 0.2f;
    int axisMask           = 63;
    m_motionProxy->setPosition(chainName, space, position, fractionMaxSpeed, axisMask);
}
